#ifndef DSO_HESSIAN_BLOCKS_H
#define DSO_HESSIAN_BLOCKS_H

const int MAX_ACTIVE_FRAMES=100;

#include <vector>
#include <iostream>
#include <fstream>

#include "dso/util/GlobalCalib.h"
#include "dso/util/NumType.h"
#include "dso/FullSystem/Residuals.h"
#include "dso/OptimizationBackend/EnergyFunctionalStructs.h"
#include "dso/util/ImageAndExposure.h"
#include "dso/util/Settings.h"
#include "dso/util/MinimalImage.h"

namespace dso
{

inline Vec2 affFromTo ( const Vec2 &from, const Vec2 &to ) // contains affine parameters as XtoWorld.
{
    return Vec2 ( from[0] / to[0], ( from[1] - to[1] ) / to[0] );
}

// forward declare
struct FrameHessian;
struct PointHessian;
class ImmaturePoint;
class FrameShell;
class EFFrame;
class EFPoint;

// 常量定义，主要是各分量的扰动大小
const float SCALE_IDEPTH=1.0f;       // scales internal value to idepth.
const float SCALE_XI_ROT=1.0f;       // 旋转部分的扰动因子
const float SCALE_XI_TRANS=0.5f;     // 平移部分扰动的因子
const float SCALE_F=50.0f;
const float SCALE_C=50.0f;
const float SCALE_W=1.0f;
const float SCALE_A=10.0f;           // a (曝光参数)的扰动因子
const float SCALE_B=1000.0f;         // b (曝光参数)的扰动因子

// inverse version
const float SCALE_IDEPTH_INVERSE= ( 1.0f / SCALE_IDEPTH );
const float SCALE_XI_ROT_INVERSE= ( 1.0f / SCALE_XI_ROT );
const float SCALE_XI_TRANS_INVERSE= ( 1.0f / SCALE_XI_TRANS );
const float SCALE_F_INVERSE= ( 1.0f / SCALE_F );
const float SCALE_C_INVERSE= ( 1.0f / SCALE_C );
const float SCALE_W_INVERSE= ( 1.0f / SCALE_W );
const float SCALE_A_INVERSE= ( 1.0f / SCALE_A );
const float SCALE_B_INVERSE= ( 1.0f / SCALE_B );


// Frame to Frame 
// 帧到帧的一些预计算结果
struct FrameFramePrecalc
{
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    void set(FrameHessian* host, FrameHessian* target, CalibHessian* HCalib);
    
    // static values
    static int instanceCounter;
    FrameHessian* host =nullptr; // defines row
    FrameHessian* target =nullptr;   // defines column

    // precalc values
    Mat33f PRE_RTll;
    Mat33f PRE_KRKiTll;
    Mat33f PRE_RKiTll;
    Mat33f PRE_RTll_0;

    Vec2f PRE_aff_mode;
    float PRE_b0_mode;

    Vec3f PRE_tTll;
    Vec3f PRE_KtTll;
    Vec3f PRE_tTll_0;

    float distanceLL;
};

/** @brief Frame with Hessian info. used as basic elements in full system
 *  带有 Hessian 的帧信息
 */
struct FrameHessian {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    inline FrameHessian() {
        instanceCounter++;
        flaggedForMarginalization=false;
        frameID = -1;
        efFrame = 0;
        frameEnergyTH = 8*8*patternNum;
        debugImage=0;
    };

    inline ~FrameHessian() {
        assert ( efFrame==0 );
        release();
        instanceCounter--;
        for ( int i=0; i<pyrLevelsUsed; i++ ) {
            delete[] dIp[i];
            delete[]  absSquaredGrad[i];
        }
        if ( debugImage != 0 ) {
            delete debugImage;
        }
    };

    // Functions
    // accessors
    EIGEN_STRONG_INLINE const SE3 &get_worldToCam_evalPT() const {
        return worldToCam_evalPT;
    }
    EIGEN_STRONG_INLINE const Vec10 &get_state_zero() const {
        return state_zero;
    }
    EIGEN_STRONG_INLINE const Vec10 &get_state() const {
        return state;
    }
    EIGEN_STRONG_INLINE const Vec10 &get_state_scaled() const {
        return state_scaled;
    }
    
    // state - state0
    EIGEN_STRONG_INLINE const Vec10 get_state_minus_stateZero() const {
        return get_state() - get_state_zero();
    }

    inline Vec6 w2c_leftEps() const {
        return get_state_scaled().head<6>();
    }
    inline AffLight aff_g2l() const {
        return AffLight ( get_state_scaled() [6], get_state_scaled() [7] );
    }
    inline AffLight aff_g2l_0() const {
        return AffLight ( get_state_zero() [6]*SCALE_A, get_state_zero() [7]*SCALE_B );
    }

    void setStateZero ( const Vec10 &state_zero );

    inline void setState ( const Vec10 &state ) {

        this->state = state;
        state_scaled.segment<3> ( 0 ) = SCALE_XI_TRANS * state.segment<3> ( 0 );
        state_scaled.segment<3> ( 3 ) = SCALE_XI_ROT * state.segment<3> ( 3 );
        state_scaled[6] = SCALE_A * state[6];
        state_scaled[7] = SCALE_B * state[7];
        state_scaled[8] = SCALE_A * state[8];
        state_scaled[9] = SCALE_B * state[9];

        PRE_worldToCam = SE3::exp ( w2c_leftEps() ) * get_worldToCam_evalPT();
        PRE_camToWorld = PRE_worldToCam.inverse();
    };

    inline void setStateScaled ( const Vec10 &state_scaled ) {

        this->state_scaled = state_scaled;
        state.segment<3> ( 0 ) = SCALE_XI_TRANS_INVERSE * state_scaled.segment<3> ( 0 );
        state.segment<3> ( 3 ) = SCALE_XI_ROT_INVERSE * state_scaled.segment<3> ( 3 );
        state[6] = SCALE_A_INVERSE * state_scaled[6];
        state[7] = SCALE_B_INVERSE * state_scaled[7];
        state[8] = SCALE_A_INVERSE * state_scaled[8];
        state[9] = SCALE_B_INVERSE * state_scaled[9];

        PRE_worldToCam = SE3::exp ( w2c_leftEps() ) * get_worldToCam_evalPT();
        PRE_camToWorld = PRE_worldToCam.inverse();
        //setCurrentNullspace();
    };

    inline void setEvalPT ( const SE3 &worldToCam_evalPT, const Vec10 &state ) {

        this->worldToCam_evalPT = worldToCam_evalPT;
        setState ( state );
        setStateZero ( state );
    };

    // set the pose Tcw 
    inline void setEvalPT_scaled ( const SE3 &worldToCam_evalPT, const AffLight &aff_g2l ) {
        Vec10 initial_state = Vec10::Zero();
        initial_state[6] = aff_g2l.a;
        initial_state[7] = aff_g2l.b;
        this->worldToCam_evalPT = worldToCam_evalPT;
        setStateScaled ( initial_state );
        setStateZero ( this->get_state() );
    };

    // clear all data
    void release();

    // 创建图像
    /** @brief create the images and graident from original color image
     * @param [in] color the input color image
     * @param [in] HCalib camera intrinsics with hessian
     */
    void makeImages ( float* color, CalibHessian* HCalib );

    // 获得先验状态
    inline Vec10 getPrior() {
        Vec10 p =  Vec10::Zero();
        if ( frameID==0 ) {
            // 第0帧
            p.head<3>() = Vec3::Constant ( setting_initialTransPrior );
            p.segment<3> ( 3 ) = Vec3::Constant ( setting_initialRotPrior );
            if ( setting_solverMode & SOLVER_REMOVE_POSEPRIOR ) {
                p.head<6>().setZero();
            }

            p[6] = setting_initialAffAPrior;
            p[7] = setting_initialAffBPrior;
        } else {
            // 中间帧
            if ( setting_affineOptModeA < 0 ) {
                p[6] = setting_initialAffAPrior;
            } else {
                p[6] = setting_affineOptModeA;
            }

            if ( setting_affineOptModeB < 0 ) {
                p[7] = setting_initialAffBPrior;
            } else {
                p[7] = setting_affineOptModeB;
            }
        }
        p[8] = setting_initialAffAPrior;
        p[9] = setting_initialAffBPrior;
        return p;
    }


    inline Vec10 getPriorZero() {
        return Vec10::Zero();
    }

    // Data
    // Keyframe index 关键帧id
    int frameID;                        // incremental ID for keyframes only!
    // instance counter 引用计数
    static int instanceCounter;
    // 自己的id
    int idx;

    EFFrame* efFrame;

    // constant info & pre-calculated values
    FrameShell* shell;
    
    // 每层金字塔的原始图像和梯度图像
    // dIp[i] 是每层金字塔 [0] 为原始分辨率，[1]为dx,[2]为dy 
    // by default, we have 6 pyramids, so we have dIp[0...5]
    // dIp[i][0] represents the grayscale pyramid, and dIp[i][1] is dx, dIp[i][2] is dy 
    // they are created in makeImages
    Eigen::Vector3f* dIp[PYR_LEVELS];    // coarse tracking / coarse initializer. NAN in [0] only.
    
    // 梯度平方和
    float* absSquaredGrad[PYR_LEVELS];  // only used for pixel select (histograms etc.). no NAN.
    
    // dI = dIp[0], the first pyramid 
    Eigen::Vector3f* dI;                 // trace, fine tracking. Used for direction select (not for gradient histograms etc.)
    

    // Photometric Calibration Stuff
    float frameEnergyTH;    // set dynamically depending on tracking residual
    float ab_exposure;  // the exposure time // 曝光时间

    bool flaggedForMarginalization;

    // 该帧活跃的地图点
    std::vector<PointHessian*> pointHessians;               // contains all ACTIVE points.

    // 已经边缘化的地图点
    std::vector<PointHessian*> pointHessiansMarginalized;   // contains all MARGINALIZED points (= fully marginalized, usually because point went OOB.)

    // outlier(弃用的)地图点
    std::vector<PointHessian*> pointHessiansOut;        // contains all OUTLIER points (= discarded.).

    // 未成熟的地图点
    std::vector<ImmaturePoint*> immaturePoints;     // contains all OUTLIER points (= discarded.).

    Mat66 nullspaces_pose;
    Mat42 nullspaces_affine;
    Vec6 nullspaces_scale;

    // variable info.
    SE3 worldToCam_evalPT;  // Tcw (in ORB-SLAM's framework)
    
    // 状态变量，[0-5]为SE3上李代数，6-7为曝光参数a,b
    Vec10 state;        // [0-5: worldToCam-leftEps. 6-7: a,b]
    
    Vec10 step;
    Vec10 step_backup;
    Vec10 state_backup;
    Vec10 state_zero;
    Vec10 state_scaled;

    // precalc values
    SE3 PRE_worldToCam; // TCW
    SE3 PRE_camToWorld; // TWC
    std::vector<FrameFramePrecalc,Eigen::aligned_allocator<FrameFramePrecalc>> targetPrecalc;   // 关键帧到关键帧之间的变换结果
    MinimalImageB3* debugImage;

};


/** @brief Camera intrinsics with Hessian
 * 相机参数主要是fx, fy, cx, cy 这四个，所以value中用的是Vec4f
 */
struct CalibHessian {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    inline CalibHessian() {
        VecC initial_value = VecC::Zero();
        initial_value[0] = fxG[0];
        initial_value[1] = fyG[0];
        initial_value[2] = cxG[0];
        initial_value[3] = cyG[0];

        setValueScaled ( initial_value );
        value_zero = value;
        value_minus_value_zero.setZero();

        instanceCounter++;
        for ( int i=0; i<256; i++ ) {
            Binv[i] = B[i] = i;    // set gamma function to identity
        }
    };

    inline ~CalibHessian() {
        instanceCounter--;
    }

    // normal mode: use the optimized parameters everywhere!
    inline float& fxl() {
        return value_scaledf[0];
    }
    inline float& fyl() {
        return value_scaledf[1];
    }
    inline float& cxl() {
        return value_scaledf[2];
    }
    inline float& cyl() {
        return value_scaledf[3];
    }

    inline float& fxli() {
        return value_scaledi[0];
    }
    inline float& fyli() {
        return value_scaledi[1];
    }
    inline float& cxli() {
        return value_scaledi[2];
    }
    inline float& cyli() {
        return value_scaledi[3];
    }

    inline void setValue ( const VecC &value ) {
        // [0-3: Kl, 4-7: Kr, 8-12: l2r]
        this->value = value;
        value_scaled[0] = SCALE_F * value[0];
        value_scaled[1] = SCALE_F * value[1];
        value_scaled[2] = SCALE_C * value[2];
        value_scaled[3] = SCALE_C * value[3];

        this->value_scaledf = this->value_scaled.cast<float>();
        this->value_scaledi[0] = 1.0f / this->value_scaledf[0];
        this->value_scaledi[1] = 1.0f / this->value_scaledf[1];
        this->value_scaledi[2] = - this->value_scaledf[2] / this->value_scaledf[0];
        this->value_scaledi[3] = - this->value_scaledf[3] / this->value_scaledf[1];
        this->value_minus_value_zero = this->value - this->value_zero;
    };

    inline void setValueScaled ( const VecC &value_scaled ) {
        this->value_scaled = value_scaled;
        this->value_scaledf = this->value_scaled.cast<float>();
        value[0] = SCALE_F_INVERSE * value_scaled[0];
        value[1] = SCALE_F_INVERSE * value_scaled[1];
        value[2] = SCALE_C_INVERSE * value_scaled[2];
        value[3] = SCALE_C_INVERSE * value_scaled[3];

        this->value_minus_value_zero = this->value - this->value_zero;
        this->value_scaledi[0] = 1.0f / this->value_scaledf[0];
        this->value_scaledi[1] = 1.0f / this->value_scaledf[1];
        this->value_scaledi[2] = - this->value_scaledf[2] / this->value_scaledf[0];
        this->value_scaledi[3] = - this->value_scaledf[3] / this->value_scaledf[1];
    };

    EIGEN_STRONG_INLINE float getBGradOnly ( float color ) {
        int c = color+0.5f;
        if ( c<5 ) {
            c=5;
        }
        if ( c>250 ) {
            c=250;
        }
        return B[c+1]-B[c];
    }

    EIGEN_STRONG_INLINE float getBInvGradOnly ( float color ) {
        int c = color+0.5f;
        if ( c<5 ) {
            c=5;
        }
        if ( c>250 ) {
            c=250;
        }
        return Binv[c+1]-Binv[c];
    }

    static int instanceCounter;

    VecC value_zero;
    VecC value_scaled;      // [fx, fy, cx, cy] in double
    VecCf value_scaledf;    // [fx, fy, cx, cy] in float
    VecCf value_scaledi;    // inverse of value_scaledf
    VecC value;
    VecC step;
    VecC step_backup;
    VecC value_backup;
    VecC value_minus_value_zero;

    // gamma function, by default from 0 to 255
    // 伽马值
    float B[256];
    float Binv[256];
};

struct PointHessian {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    enum PtStatus {ACTIVE=0, INACTIVE, OUTLIER, OOB, MARGINALIZED}; // Status, OOB=Out Of Bundary?
    
    PtStatus status;
    
    // Create a Point from immature point 
    PointHessian ( const ImmaturePoint* const rawPoint, CalibHessian* Hcalib );
    
    inline ~PointHessian() {
        assert ( efPoint==0 );
        release();
        instanceCounter--;
    }
    
    inline void setPointStatus ( PtStatus s ) {
        status=s;
    }


    inline void setIdepth ( float idepth ) {
        this->idepth = idepth;
        this->idepth_scaled = SCALE_IDEPTH * idepth;
    }
    inline void setIdepthScaled ( float idepth_scaled ) {
        this->idepth = SCALE_IDEPTH_INVERSE * idepth_scaled;
        this->idepth_scaled = idepth_scaled;
    }
    inline void setIdepthZero ( float idepth ) {
        idepth_zero = idepth;
        idepth_zero_scaled = SCALE_IDEPTH * idepth;
        nullspaces_scale = - ( idepth*1.001 - idepth/1.001 ) *500;
    }
    
    void release()
    {
        for ( auto r: residuals )
            delete r;
        residuals.clear();
    }

    inline bool isOOB ( const std::vector<FrameHessian*>& toKeep, const std::vector<FrameHessian*>& toMarg ) const {

        int visInToMarg = 0;
        for ( PointFrameResidual* r : residuals ) {
            if ( r->state_state != ResState::IN ) {
                continue;
            }
            for ( FrameHessian* k : toMarg )
                if ( r->target == k ) {
                    visInToMarg++;
                }
        }
        if ( ( int ) residuals.size() >= setting_minGoodActiveResForMarg &&
                numGoodResiduals > setting_minGoodResForMarg+10 &&
                ( int ) residuals.size()-visInToMarg < setting_minGoodActiveResForMarg ) {
            return true;
        }

        if ( lastResiduals[0].second == ResState::OOB ) {
            return true;
        }
        if ( residuals.size() < 2 ) {
            return false;
        }
        if ( lastResiduals[0].second == ResState::OUTLIER && lastResiduals[1].second == ResState::OUTLIER ) {
            return true;
        }
        return false;
    }


    inline bool isInlierNew() {
        return ( int ) residuals.size() >= setting_minGoodActiveResForMarg
               && numGoodResiduals >= setting_minGoodResForMarg;
    }
    
    static int instanceCounter;             // 实例计数
    EFPoint* efPoint =nullptr;              // energy 

    FrameHessian* host;     // the host frame 
    float u,v;              // pixel position
    int idx;
    float energyTH;
    bool hasDepthPrior;         // 是否有先验？
    float my_type;
    float idepth_scaled;
    float idepth_zero_scaled;
    float idepth_zero;
    float idepth;
    float step;
    float step_backup;
    float idepth_backup;
    float nullspaces_scale;
    float idepth_hessian =0;
    float maxRelBaseline =0;
    int numGoodResiduals =0;
    
    // 点到帧的残差
    std::vector<PointFrameResidual*> residuals;                 // only contains good residuals (not OOB and not OUTLIER). Arbitrary order.
    
    // 最近的两个帧的残差，[0]是最近的，[1]是第二近的
    std::pair<PointFrameResidual*, ResState> lastResiduals[2];  // contains information about residuals to the last two (!) frames. ([0] = latest, [1] = the one before).
    
    // static values
    float color[MAX_RES_PER_POINT];         // colors in host frame
    float weights[MAX_RES_PER_POINT];       // host-weights for respective residuals.

};



}

#endif
